#! usr/bin/env python

import rospy
from std_msgs.msg import Float64
if __name__ == "__main__":
    #使用python实现消息发布

    # 1.导入包
    # 2.初始化ros节点
    # 3.创建发布者对象
    # 4.编写发布逻辑和数据

    rospy.init_node("gazebo_homework") 

    #定义数组存储机器人正向运动学数据
    lst = [[0.0,0.0,0.0],[1.4,0.7,-0.7],[-1.0,-0.5,0.5],[2.0,0.6,-0.5],[0.0,0.0,0.0]]

    #这里发布三个节点的数据
    pub1 = rospy.Publisher("//robosim//joint1_position_controller//command",Float64,queue_size=10)
    pub2 = rospy.Publisher("//robosim//joint2_position_controller//command",Float64,queue_size=10)
    pub3 = rospy.Publisher("//robosim//joint3_position_controller//command",Float64,queue_size=10)
            
    #这是对应的三个信息载体        
    msg1 = Float64()
    msg2 = Float64()
    msg3 = Float64()

    #指定发布频率,传入的数字是赫兹数
    rate = rospy.Rate(0.3) 
    #定义cont
    cont = 0
    while not rospy.is_shutdown():
        msg1.data = lst[cont][0]
        msg2.data = lst[cont][1]
        msg3.data = lst[cont][2]
        pub1.publish(msg1)  #发布数据
        pub2.publish(msg2)
        pub3.publish(msg3)
        print("输出数据...  个数："+str(cont+1))
        #读取下一组数据
        cont += 1
        if cont >= len(lst):
            break
        #调用休眠函数
        rate.sleep()





